#include "StdAfx.h"

#include <time.h>

#include "VocoderThread.h"
#include "Locator.h"
#include "CarrierStream.h"
#include "WaveSample.h"
#include "SampleBuffer.h"
#include "PitchTracker.h"
#include "Recorder.h"
#include "Settings.h"
#include "GFFT.h"

extern "C"
{
#include "../VocoderBase/vocode.h"
}


//vocode_volume = 1.0;
static short* outputSample = NULL;
//static int modulator_max_magnitude = (1 << (16 - 1)) - 1;
//static int carrier_max_magnitude = (1 << (16 - 1)) - 1;
//static int output_max_magnitude = (1 << (16 - 1)) - 1;

VocoderThread::VocoderThread(void)
{
	isRunning = false;

	modulator1 = NULL;
	modulator2 = NULL;
	carrier1 = NULL;
	carrier2 = NULL;
	vocoded1 = NULL;
	vocoded2 = NULL;
	vocoded3 = NULL;
}

VocoderThread::~VocoderThread(void)
{
	DeleteFirstBuffers();
	if(modulator2) { delete modulator2; }
	if(carrier2) delete carrier2;
	if(vocoded2) delete vocoded2;
	if(vocoded3) delete vocoded3;
}

void VocoderThread::InitFirstBuffers()
{
	int half = vocode_window_length/2;
	modulator1 = new Sample(half);
	carrier1 = new Sample(half);
	vocoded1 = new Sample(vocode_window_length);
}
void VocoderThread::DeleteFirstBuffers()
{
	//if(modulator1)
	//{
	//	delete modulator1;
	//	modulator1 = NULL;
	//}
	if(carrier1)
	{
		delete carrier1;
		carrier1 = NULL;
	}
	if(vocoded1)
	{
		delete vocoded1;
		vocoded1 = NULL;
	}
}

/*
Sample* VocoderThread::CrossFade(Sample* old, Sample* cur, Sample* next)
{
	Sample* sample = new Sample(cur->length);
	register int i=0, sampleIdx=0, half=cur->length/2;
	for(; i<half; i++, sampleIdx++)
	{
		short sc = cur->data[i];
		double c = (sc*(i/(double)half));//increasing cur
		short so = old->data[cur->length-half+i];
		double o = (so*((half-i)/(double)half));//decreasing old
		short s = (short) (c+o); 			 
		sample->data[sampleIdx] = s;
	}	
	for(i = 0; i<half; i++, sampleIdx++)
	{
		short s = (short) ((next->data[i]*(i/(double)half)) + //increasing next
			(cur->data[cur->length-half+i]*((half-i)/(double)half))); //decreasing cur
		sample->data[sampleIdx] = s;
	}	
	return sample;
}
*/

void VocoderThread::Run()
{
	isRunning = true;
	vocode_normalize = Locator::Instance()->GetSettings()->GetVocoder()->normalize;
	InitFirstBuffers();
	while(isRunning)
	{
		FillBuffers();
	}
}

void VocoderThread::Stop()
{
	isRunning = false;
	Thread::Terminate();
}

static int totalGetDataTime = 0;
static int countGetData = 0;

void VocoderThread::FillBuffers()
{
	Locator* locator = Locator::Instance();
	locator->GetSettingsProvider()->UpdateSettings(locator->GetSettings());
	switch(locator->GetSettings()->GetPlayer()->streamType)
	{
	case Settings::PlayerSettings::StreamType::Modulator:
		FillWithModulator();
		break;
	case Settings::PlayerSettings::StreamType::Carrier:
		FillWithCarrier();
		break;
	case Settings::PlayerSettings::StreamType::Vocoder:
		FillWithVocoder();
		break;
	}
}

void VocoderThread::FillWithModulator()
{
	Locator* locator = Locator::Instance();
	SampleBuffer2* modBuffer = locator->GetModulatorBuffer();
	Sample* m1 = modBuffer->Read();
	Sample* m2 = modBuffer->Read();
	Sample* m = m1->Append(m2);
	if(locator->GetSettings()->GetPlayer()->shouldRecord)
	{
		locator->GetRecorder()->AddModulatorSample(m);
	}
	modBuffer->Recycle(m1);
	modBuffer->Recycle(m2);
	locator->GetVocoderBuffer()->Write(m);
	delete m;
}

void VocoderThread::FillWithCarrier()
{
	Locator* locator = Locator::Instance();
	Sample* c1 = locator->GetCarrierStream()->GetData();
	Sample* c2 = locator->GetCarrierStream()->GetData();
	Sample* c = c1->Append(c2);
	if(locator->GetSettings()->GetPlayer()->shouldRecord)
	{
		locator->GetRecorder()->AddCarrierSample(c);
	}
	locator->GetVocoderBuffer()->Write(c);
	delete c1;
	delete c2;
	delete c;
}

void VocoderThread::FillWithVocoder()
{
	Sample* v1 = NextCrossFaded();
	Sample* v2 = NextCrossFaded();
	Sample* v = v1->Append(v2);
	Locator* locator = Locator::Instance();
	if(locator->GetSettings()->GetPlayer()->shouldRecord)
	{
		locator->GetRecorder()->AddVocoderSample(v);
	}
	locator->GetVocoderBuffer()->Write(v);
	delete v1;
	delete v2;
	delete v;
}

static void ApplySilence(Sample* prevModulator, Sample* modulator, Sample* vocoded)
{
	//find quiet periods in modulator and silence the same periods in carrier
	Settings::VocoderSettings* vsettings = Locator::Instance()->GetSettings()->GetVocoder();
	int threshold = vsettings->absSilenceThreshold;
	int minSamples = vsettings->minSilenceSamples;
	Sample* modSequence = prevModulator->Append(modulator);
	int startIdx = -1;
	for(int i=0; i<modSequence->length; i++)
	{		
		if(abs(modSequence->data[i])<threshold)
		{
			if(startIdx==-1)
			{
				startIdx = i;
			}
		}
		else
		{
			if(i-startIdx>minSamples && startIdx!=-1)
			{
				for(int j=startIdx; j<i+750 && j<modSequence->length; j++)
				{
					if(j>prevModulator->length)
					{
						vocoded->data[j-prevModulator->length] = modSequence->data[j];
					}
				}
			}
			startIdx = -1;
		}
	}
	delete modSequence;
}

static void MixWithModulator(Sample* modulator, Sample* sample)
{
	for(int i=0; i<modulator->length; i++)
	{
		sample->data[i] = (sample->data[i]+modulator->data[i])/2;
	}
}

static void UseModulatorAmplitude(Sample* modulator, Sample* sample)
{
	short max = 0;
	for(int i=0; i<modulator->length; i++)
	{
		if(abs(modulator->data[i])>max) {
			max = abs(modulator->data[i]);
		}
	}

	for(int i=0; i<modulator->length; i++)
	{
		double mult =  (double) modulator->data[i] / (double)max ;
		if(mult<0) mult *= -1;
		//mult = (mult+2.0)/3.0;
		sample->data[i] *= mult;
		//sample->data[i] = modulator->data[i]+sample->data[i]*2 / 3;
	}
}

static GFFT<256, 7> gfft;
static double fftAr[256];
static bool fftArInitialized = false;

static void DoFFTImpl(short* modulator, short* sample, int length)
{
	//GFFT<512, 9> gfft;

	for(int i=0; i<length; i++) {
		fftAr[i] = (double)modulator[i];
	}
	gfft.fft(fftAr);

	double total = 0;
	double max = 0;
	for(int i=0; i<length/2; i++) {
		double d = fftAr[i];//abs(fftAr[i]);
		total += d;
		if(abs(d)>max) {
			max = abs(d);
		}
	}
	double avg = total / length/2;

	double sum = 0;
	for(int i=0; i<length/2; i++) {
		sum += pow(fftAr[i]-avg, 2);
	}
	double stddev = sqrt(sum / (length/2-1));
	if(max < (4*stddev)) {
		//use modulator data
		for(int i=0; i<length; i++) {
			sample[i] = modulator[i];
		}
	}
}
static void DoFFT(Sample* modulator, Sample* sample)
{
	DoFFTImpl(modulator->data, sample->data, 256);
	int pos = /*sizeof(short)*/256;
	DoFFTImpl(modulator->data+pos, sample->data+pos, 256);
	pos += 256;
	DoFFTImpl(modulator->data+pos, sample->data+pos, 256);
	pos += 256;
	DoFFTImpl(modulator->data+pos, sample->data+pos, 256);
}


static Sample* prevModulator = NULL;
Sample* VocoderThread::NextCrossFaded()
{
	Locator* locator = Locator::Instance();
	Settings* settings = locator->GetSettings();
	bool shouldRecord = settings->GetPlayer()->shouldRecord;
	Recorder* recorder = locator->GetRecorder();
	SampleBuffer2* modBuffer = locator->GetModulatorBuffer();
	modulator2 = modBuffer->Read();
	if(shouldRecord)
	{
		locator->GetRecorder()->AddModulatorSample(modulator2);
	}
	PitchAnalysis analysis;
	Settings::PitchTrackerSettings* psettings = settings->GetPitchTracker();
	locator->GetPitchTracker()->Analyze(modulator2, psettings->amplitudeThreshold, psettings->noiseThreshold, &analysis);
	if(shouldRecord)
	{
		recorder->AddPitchTrackerSample(analysis.pitchOut);
	}
	else {
		delete analysis.pitchOut;
	}
	Sample* modulator = modulator1->Append(modulator2);

	carrier2 = locator->GetCarrierStream()->GetData(analysis.frequency, analysis.voiced);
	//ModulateCarrier(modulator2, carrier2);

	if(shouldRecord)
	{
		recorder->AddCarrierSample(carrier2);
	}
	//carrier2 = locator->GetCarrierStream()->GetData();
	Sample* carrier = carrier1->Append(carrier2);
	
	vocoded2 = new Sample(carrier->length);
	vocode_window_sample(modulator->data, carrier->data, vocoded2->data, NULL);
	
	if(settings->GetVocoder()->applySilence)
	{
		if(prevModulator==NULL)
		{
			prevModulator = new Sample(modulator->length);
		}
		ApplySilence(prevModulator, modulator, vocoded2);
		delete prevModulator;
		prevModulator = modulator->Copy(1.0, 0);
	}
	if(settings->GetVocoder()->mixWithModulator)
	{
		MixWithModulator(modulator, vocoded2);
	}
	if(settings->GetVocoder()->useModulatorAmplitude)
	{
		//UseModulatorAmplitude(modulator, vocoded2);
		DoFFT(modulator, vocoded2);
	}
	
	


	Sample* vocoded = CrossFade(vocoded1, vocoded2);

	modBuffer->Recycle(modulator1);
	modulator1 = modulator2;
	modulator2 = NULL;
	delete carrier1;
	carrier1 = carrier2;
	carrier2 = NULL;
	delete vocoded1;
	vocoded1 = vocoded2;
	vocoded2 = NULL;

	delete modulator;
	delete carrier;
	return vocoded;
}

Sample* VocoderThread::CrossFade(Sample* prev, Sample* next)
{
	register int i=0, overlap=next->length/4;
	Sample* sample = new Sample(next->length/2);
	for(; i<overlap; i++)
	{
		short sc = next->data[i];
		double c = (sc*(i/(double)overlap));//increasing cur
		short so = prev->data[next->length-overlap+i];
		double o = (so*((overlap-i)/(double)overlap));//decreasing old
		short s = (short) (c+o); 			 
		sample->data[i] = s;
	}	
	for(int i=0; i<overlap; i++)
	{
		sample->data[overlap+i] = next->data[overlap+i];
	}
	return sample;
}

